C--------------------------------------------------------- 
C		    三节点常应变CST单元 UEL子程序
C			公众号：易木木响叮当		B站：易木木响叮当
C--------------------------------------------------------- 
      SUBROUTINE UEL(RHS,AMATRX,SVARS,ENERGY,NDOFEL,NRHS,NSVARS,
     1 PROPS,NPROPS,COORDS,MCRD,NNODE,U,DU,V,A,JTYPE,TIME,DTIME,
     2 KSTEP,KINC,JELEM,PARAMS,NDLOAD,JDLTYP,ADLMAG,PREDEF,NPREDF,
     3 LFLAGS,MLVARX,DDLMAG,MDLOAD,PNEWDT,JPROPS,NJPROP,PERIOD)
C
       INCLUDE 'ABA_PARAM.INC'
C      IMPLICIT REAL*8(A-H,O-Z)
C      IMPLICIT INTEGER(I,J,K,L,M,N)
C
      DIMENSION RHS(MLVARX,*),AMATRX(NDOFEL,NDOFEL),PROPS(*),
     1 SVARS(*),ENERGY(8),COORDS(MCRD,NNODE),U(NDOFEL),
     2 DU(MLVARX,*),V(NDOFEL),A(NDOFEL),TIME(2),PARAMS(*),
     3 JDLTYP(MDLOAD,*),ADLMAG(MDLOAD,*),DDLMAG(MDLOAD,*),
     4 PREDEF(2,NPREDF,NNODE),LFLAGS(*),JPROPS(*)
 
 
      REAL*8 JMATRX,JINVER
      DIMENSION DMATRX(3,3),BMATRX(3,6),A_MATRX(3,4),G_MATRX(4,6),
     1            dNdrs(2,3),JMATRX(2,2),JINVER(2,2)
      PARAMETER (zero=0.0d0,one=1.0d0,two = 2.0D0,THREE=3.0d0)
 
!			矩阵置零
			CALL K_MATRIX_ZERO (RHS,NDOFEL,NRHS)
			CALL K_MATRIX_ZERO (AMATRX,NDOFEL,NDOFEL)
			CALL K_MATRIX_ZERO (A_MATRX,3,4)
			CALL K_MATRIX_ZERO (G_MATRX,4,6)
			CALL K_MATRIX_ZERO (BMATRXX,3,6)
 			CALL K_MATRIX_ZERO (dNdrs,2,3)
C     求D矩阵
      YM = PROPS(1)
      POIS = PROPS(2)
      THICK = PROPS(3)      
!      write(6,*)'DMATRX============'
!      do i = 1, 3
!         write(6,"(*(g0,1X))") DMATRX(i,:) 
!      end do
C     形函数对rs的偏导
      dNdrs(1,1)  = one
      dNdrs(1,2)  = zero
      dNdrs(1,3)  =-one
      dNdrs(2,1)  = zero
      dNdrs(2,2)  = one
      dNdrs(2,3)  =-one
c     雅克比矩阵、行列式和逆矩阵
      JMATRX = MATMUL(dNdrs,TRANSPOSE(COORDS(1:2,1:NNODE)))        
      CALL K_MATRIX_Det(JMATRX,2,DETJ)
      CALL K_MATRIX_Inv(JMATRX,JINVER,2)       
c     构造B矩阵
! ----------------------------------------------------------------------
!			计算A矩阵
      A_MATRX(1,1) = JMATRX(2,2)
      A_MATRX(1,2) = -JMATRX(1,2)
      A_MATRX(2,3) = -JMATRX(2,1)
      A_MATRX(2,4) = JMATRX(1,1)
      A_MATRX(3,1) = -JMATRX(2,1)
      A_MATRX(3,2) = JMATRX(1,1)
      A_MATRX(3,3) = JMATRX(2,2)
      A_MATRX(3,4) = -JMATRX(1,2)
      A_MATRX = A_MATRX/DETJ
!!			计算G矩阵      
      G_MATRX(1,1) = dNdrs(1,1)
			G_MATRX(1,3) = dNdrs(1,2)
			G_MATRX(1,5) = dNdrs(1,3)
			G_MATRX(2,1) = dNdrs(2,1)
			G_MATRX(2,3) = dNdrs(2,2)
			G_MATRX(2,5) = dNdrs(2,3)
			G_MATRX(3,2) = dNdrs(1,1)
			G_MATRX(3,4) = dNdrs(1,2)
			G_MATRX(3,6) = dNdrs(1,3)
			G_MATRX(4,2) = dNdrs(2,1)
			G_MATRX(4,4) = dNdrs(2,2)
			G_MATRX(4,6) = dNdrs(2,3)
!			计算B矩阵 [B]=[A][G]
			BMATRX = MATMUL(A_MATRX,G_MATRX)
!			刚度矩阵计算 
			call cal_D(1,DMATRX,YM,POIS)    			     
      AREA=0.5*DETJ  	! 三角形单元面积=1/2 det(J)
      AMATRX = AMATRX+AREA*THICK*MATMUL(MATMUL(TRANSPOSE(BMATRX),DMATRX),BMATRX)
!			残余力计算
      CALL K_MATRIX_Sub(AMATRX,U,RHS,NDOFEL,NDOFEL,NRHS)
 
      RETURN
      END

      subroutine K_MATRIX_ZERO (A,n,m)
C			get zero matrix
      INCLUDE 'ABA_PARAM.INC'
      dimension A(n,m)
      do i=1,n
          do j=1,m
              A(i,j) = 0.d0
          end do
      end do
      return
      end 
     
      subroutine K_MATRIX_Sub(A,B,C,l,n,m)
      INCLUDE 'ABA_PARAM.INC'
      dimension A(l,n), B(n,m), C(l,m)
      call k_Matrix_zero(C,l,m)
      do i=1,l
          do j=1,m
              do k=1,n
                  C(i,j) = C(i,j) - A(i,k) * B (k,j)
              end do
          end do
      end do
      return
      end
      
      subroutine cal_D(Pss,D,E,ENU) 
      INCLUDE 'ABA_PARAM.INC'
      double precision E ENU D1 D2
      INTEGER Pss
      double precision, DIMENSION(3,3)::D
      PARAMETER (ZERO= 0.D0,ONE = 1.D0,TWO=2.D0)
        DO I=1,3
            DO K=1,3
              D(I,K)=ZERO
            ENDDO
        ENDDO
        IF (Pss.EQ.1) THEN
C       平面应力状态
					D1 = E/(ONE-ENU**2)
          D(1,1)=ONE*D1
          D(2,2)=ONE*D1
          D(1,2)=ENU*D1
          D(2,1)=ENU*D1
          D(3,3)=(ONE-ENU)/TWO*D1
        ELSE IF (Pss.EQ.2) THEN
C       平面应力状态
					D2 =  E*(ONE-ENU)/((ONE+ENU)*(ONE-TWO*ENU))
          D(1,1)=ONE*D2
          D(2,2)=ONE*D2
          D(1,2)=ENU/(ONE-ENU)*D2
          D(2,1)=ENU/(ONE-ENU)*D2
          D(3,3)=(ONE-TWO*ENU)/(TWO*(1-ENU))*D2
        END IF
      end 
                         
      SUBROUTINE K_MATRIX_Inv(a,c,n)   
C     get inverse matrix
      ! a(n,n) - array of coefficients for matrix A
      ! n      - dimension
      ! output ...
      ! c(n,n) - inverse matrix of A
      implicit none 
      integer,intent(in)::n
      double precision,intent(in):: a(n,n)
      double precision,intent(out):: c(n,n)
      double precision L(n,n), U(n,n), b(n), d(n), x(n)
      double precision coeff,tem_a(n,n)
      integer i, j, k
      
      tem_a = a
      L=0.0D0
      U=0.0D0
      b=0.0D0

      do k=1, n-1
          do i=k+1,n
              coeff=tem_a(i,k)/tem_a(k,k)
              L(i,k) = coeff
              do j=k+1,n
                  tem_a(i,j) = tem_a(i,j)-coeff*tem_a(k,j)          
              end do
          end do
      end do
      do i=1,n
		  L(i,i) = 1.0D0
      end do
      do j=1,n
          do i=1,j
              U(i,j) = tem_a(i,j)
          end do
      end do

      do k=1,n
          b(k)=1.0D0
          d(1) = b(1)
          do i=2,n
              d(i)=b(i)
              do j=1,i-1
                 d(i) = d(i) - L(i,j)*d(j)
              end do
          end do
		  x(n)=d(n)/U(n,n)
          do i = n-1,1,-1
              x(i) = d(i)
              do j=n,i+1,-1
			      x(i)=x(i)-U(i,j)*x(j)
              end do
          x(i) = x(i)/u(i,i)
          end do
          do i=1,n
              c(i,k) = x(i)
          end do
		  b(k)=0.0D0
      end do
		  
      return
      END
           
      SUBROUTINE K_MATRIX_Det(A,N,DET)
!	  全选主元高斯消去法(Gauss-Jordan)求矩阵的行列式       
      DIMENSION A(N,N)
      DOUBLE PRECISION A,DET,F,D,Q
	    F=1.0
	    DET=1.0
	    DO 100 K=1,N-1
	    Q=0.0
      DO 10 I=K,N
      DO 10 J=K,N
        IF (ABS(A(I,J)).GT.Q) THEN
	      Q=ABS(A(I,J))
	      IS=I
	      JS=J
        END IF
10	  CONTINUE
      IF (Q+1.0.EQ.1.0) THEN
	    DET=0.0
	    RETURN
      END IF
      IF (IS.NE.K) THEN
	    F=-F
	    DO 20 J=K,N
	      D=A(K,J)
	      A(K,J)=A(IS,J)
	      A(IS,J)=D
20	    CONTINUE
      END IF
      IF (JS.NE.K) THEN
	    F=-F
	    DO 30 I=K,N
	      D=A(I,JS)
	      A(I,JS)=A(I,K)
	      A(I,K)=D
30	    CONTINUE
      END IF
      DET=DET*A(K,K)
      DO 50 I=K+1,N
	    D=A(I,K)/A(K,K)
        DO 40 J=K+1,N
40	    A(I,J)=A(I,J)-D*A(K,J)
50	  CONTINUE
100	  CONTINUE
	    DET=F*DET*A(N,N)
	    RETURN
	    END 


                   